/*
 * Copyright (C) 2020 1km Holding Limited
 */

#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include "math.h"

#include "aos/hal/uart.h"
#include "ulog/ulog.h"
#include "lidar_flatscan.h"
#include "aos/kernel.h"
#include "../../bsp/base_type.h"
#include "../../bsp/cli_cmd.h"
#include "../../algorithm/filter.h"
#include "../../bsp/io_config.h"
#include "../../bsp/xp_uart.h"

#define BEA_POLYNOM 0x90d9

#define LOG_TAG "LIDAR"

#define PI 3.1415926
// #define DRYER
#define POINT_NUM 400


/* define dev */
static uart_dev_t lidar_uart,lidar_uart_T;
static int minDis=0,maxHigh=0;
static int minDis_T=0;
static u8 online=0;


void xp_lidar_data_deal(u8 *data,u8 len);
void xp_lidar_data_deal_new(u8 *data,u16 len);
void xp_lidar_H_W_analysis(u8 *data,int offset,int len);


/**
 * @brief 判断雷达是否故障
 * 
 * @return bool 0表示正常，1表示雷达故障
 */
bool is_lidar_error()
{
	return 0;
}

/**
 * @brief 判断雷达是否失去连接
 * 
 * @return bool 0表示连接正常，1表示失去连接。
 */
bool is_lidar_lost_connect()
{
	return 0;
}

/**
 * @brief 获取雷达最近距离
 * 
 * @return int16_t 返回雷达最近距离，单位cm
 */
int16_t get_lidar_distance()
{
	return minDis/10;
}

/**
 * @brief 关闭雷达
 * 
 * @return int32_t int32_t 0表示关闭雷达指令发送成功，非0表示失败。
 */
int32_t lidar_close()
{

	return 0;
}

/**
 * @brief 打开雷达
 * 
 * @return int32_t 0表示打开雷达指令发送成功，非0表示失败。
 */
int32_t lidar_open()
{

	return 0;
}

/**
 * @brief 雷达是否打开
 * 
 * @return int32_t 1表示打开成功，0表示失败。
 */
int32_t is_lidar_open()
{
	return online;
}

static uint16_t lidar_crc16(uint8_t *buf, uint16_t cnt)
{
	uint16_t crc = 0;
	uint16_t i, j;
	for(i = 0; i < cnt; i++)
	{
		crc ^= (uint16_t)(buf[i] << 8); /* move byte into MSB of 16bit CRC */
		for(j = 0; j < 8; j++)
		{
			if((crc & 0x8000) != 0){
				crc = (uint16_t)((crc << 1) ^ BEA_POLYNOM);
			}else{
				crc <<= 1;
			}
		}
	}
	
	return crc;
}

static int32_t set_lidar_param()
{
	int32_t ret = 0;	
	int rx_size = 0;
	uint16_t crc_calc_value = 0;
	uint8_t get_lidar_answer[43];
	/*uint8_t set_lidar_param_cmd[37] = {0xBE, 0xA0, 0x12, 0x34, 0x02, 0x25, 0x00, 0x02, 0x00,
									   0x00, 0x00, 0x53, 0xC3, 0x00, 0x00, 0x00, 0x00, 0x00,
									   0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00,
									   0x00, 0x00, 0x30, 0x2A, 0x00, 0x00, 0x00, 0x00};*/

   uint8_t set_lidar_param_cmd[37] = {0xBE, 0xA0, 0x12, 0x34, 0x02, 0x25, 0x00, 0x02, 0x00,
									   0x00, 0x00, 0x53, 0xC3, 0x00, 0x00, 0x00, 0x01, 0x00,
									   0x00, 0x00, 0x00, 0x90, 0x01, 0x00, 0x00, 0x00, 0x00,
									   0x00, 0x00, 0x30, 0x2A, 0x00, 0x00, 0x00, 0x00};
//   uint8_t set_lidar_param_cmd[37] = {0xBE, 0xA0, 0x12, 0x34, 0x02, 0x25, 0x00, 0x02, 0x00,
//									   0x00, 0x00, 0x53, 0xC3, 0x00, 0x00, 0x00, 0x01, 0x00,
//									   0x00, 0x00, 0x00, 0x90, 0x01, 0x00, 0x00, 0x00, 0x00,
//									   0x08, 0x07, 0x28, 0x23, 0x00, 0x00, 0x00, 0x00};

	crc_calc_value = lidar_crc16(set_lidar_param_cmd, 35);
	set_lidar_param_cmd[36] = crc_calc_value >> 8;
	set_lidar_param_cmd[35] = crc_calc_value & 0xff;

	ret = hal_uart_send(&lidar_uart, set_lidar_param_cmd, 37, 50);
    if (0 != ret) {
        return -1;
    }

	ret = hal_uart_recv_II(&lidar_uart, get_lidar_answer, 43, &rx_size, 1000);
	if (ret == 0 && rx_size == 43){
		/*if(0 != get_lidar_answer[13] || 0 != get_lidar_answer[14] ||
		   0 != get_lidar_answer[15] || 0 != get_lidar_answer[16]){*/
		if(0x54 != get_lidar_answer[11] || 0xc3 != get_lidar_answer[12]){
			return -1;
		}
	}else{
		return -1;
	}

	return 0;
}

static int32_t set_lidar_single_shot()
{
	int32_t ret = 0;	
	int rx_size = 0;
	uint16_t crc_calc_value = 0;
	//uint8_t get_lidar_answer[500];
	
	uint8_t get_lidar_answer[815];
	uint8_t get_lidar_distance_cmd[16] = {0xBE, 0xA0, 0x12, 0x34, 0x02, 0x10, 0x00, 0x02, 0x00,
							  		   	  0x00, 0x00, 0x5B, 0xC3, 0x00, 0x89, 0xB1};

	ret = hal_uart_send(&lidar_uart, get_lidar_distance_cmd, 16, 10);
    if (0 != ret) {
        return -1;
    }

	/*ret = hal_uart_recv_II(&lidar_uart, get_lidar_answer, 500, &rx_size, 100);
	if (ret == 0 && rx_size == 215){*/
	ret = hal_uart_recv_II(&lidar_uart, get_lidar_answer, 815, &rx_size, 1000);
	if (ret == 0 && rx_size == 815){
		if(0x5B != get_lidar_answer[11] || 0xC3 != get_lidar_answer[12]){
			return -1;
		}
	}else{
		return -1;
	}

	return 0;
}


static int32_t store_lidar_param()
{
	int32_t ret = 0;	
	int rx_size = 0;
	uint16_t crc_calc_value = 0;
	uint8_t get_lidar_answer[15];
	uint8_t store_lidar_param_cmd[15] = {0xBE, 0xA0, 0x12, 0x34, 0x02, 0x0F, 0x00, 0x02, 0x00,
									     0x00, 0x00, 0x55, 0xC3, 0x6D, 0x27};

	ret = hal_uart_send(&lidar_uart, store_lidar_param_cmd, 37, 50);
    if (0 != ret) {
        return -1;
    }

	ret = hal_uart_recv_II(&lidar_uart, get_lidar_answer, 15, &rx_size, 1000);
	if (ret == 0 && rx_size == 15){
		if(0x55 != get_lidar_answer[11] || 0xC3 != get_lidar_answer[12]){
			return -1;
		}
	}else{
		return -1;
	}

	return 0;
}

static int32_t set_lidar_baud_rate()
{

	return 0;
}

static int32_t calc_lidar_closest_distance(uint8_t *buf, uint16_t cnt)
{	
	return 0;
}

static int32_t is_lidar_data_invalid(u8 *data)
{
	u16 distance = 0;
	double angle,C,A;
	int i = 0;
	int err_count = 0;
		
	for(i; i < 5; i++)
	{
		memcpy(&distance,data+(55+i)*2,2);
		angle=5.4 + (i+5)*1.08;
		A=angle*PI/180.0;
		C=cos(A);
		distance=C*distance;
		if(distance > 1500){
			err_count++;
		}
	}
	if(5 == err_count){
		return 1;
	}

	return 0;
}

/**************************************************************************
                    高斯滤波
*************************************************************************/
void GaussFilter_new(u8* data)
{
  u16 templates[3] = { 2,4,2 };  //gauss核
  u16 tempdata[100*4];
  int sum,index;
  u16 i,j;
  	for(i=0;i<100*4;i++){
		memcpy(&tempdata[i],data+i*2,2); //数据转为short型
  		}
	
	for(i=0;i<98+300;i++)
		{
		 sum=0;
		 index=0;
		 for(j=i;j<i+3;j++)
		 	{
		 	 sum+=tempdata[j]*templates[index++];
		 	}
		 tempdata[i]=sum/8;
		}
	memcpy(data,tempdata,200*4);
}

void GaussFilter(u8* data)
{
  u16 templates[3] = { 2,4,2 };  //gauss核
  u16 tempdata[100];
  int sum,index;
  u16 i,j;
  	for(i=0;i<100;i++){
		memcpy(&tempdata[i],data+i*2,2); //数据转为short型
  		}
	
	for(i=0;i<98;i++)
		{
		 sum=0;
		 index=0;
		 for(j=i;j<i+3;j++)
		 	{
		 	 sum+=tempdata[j]*templates[index++];
		 	}
		 tempdata[i]=sum/8;
		}
	memcpy(data,tempdata,200);
}

static void lidar_data_recv(void *param)
{
	int32_t ret = 0;
	int rx_size = 0;
	uint16_t crc_calc_value = 0;
	uint16_t crc_exact_value = 0;
	uint16_t point_count = 0;
	uint16_t distance = 0;
	static uint8_t err_cnt = 0;
	
	/* data buffer */
	//100点距离信息的大小
	uint8_t get_lidar_answer[215+600];
	uint8_t get_lidar_distance_cmd[16] = {0xBE, 0xA0, 0x12, 0x34, 0x02, 0x10, 0x00, 0x02, 0x00,
							  		   	  0x00, 0x00, 0x5B, 0xC3, 0x00, 0x89, 0xB1};
	int32_t start_index;
    static u8  lidarDataBuff[200*4];//高斯滤波用

    while(1){
		xp_uart_rec_clear(&lidar_uart);
		ret = hal_uart_send(&lidar_uart, get_lidar_distance_cmd, 16, 10);
	    if (0 != ret) {
	        continue;
	    }
	
		ret = hal_uart_recv_II(&lidar_uart, get_lidar_answer, 215+600, &rx_size, 1000);
		if (ret == 0 && rx_size == 215+600){
			err_cnt = 0;
			online=1;
			crc_exact_value = (get_lidar_answer[rx_size-1] << 8) | get_lidar_answer[rx_size-2];
			crc_calc_value = lidar_crc16(get_lidar_answer, rx_size-2);
			
			if(crc_calc_value == crc_exact_value){
				point_count = (get_lidar_answer[6] << 8) | get_lidar_answer[5] - 15;
				//calc_lidar_closest_distance(&get_lidar_answer[13], point_count);
				//将后面的数据排除掉35个，防止扫地面的情况；
				//还有用角度50接近平行的数据大于2米作为判断雷达扫不到车的情况
				//memcpy(&distance,&get_lidar_answer[113],2);
				//if(distance < 2000){
				#ifdef DRYER
				memcpy(lidarDataBuff,&get_lidar_answer[13],200);
				//GaussFilter(lidarDataBuff);
				GaussFilter_new(lidarDataBuff);
				// if(!is_lidar_data_invalid(&get_lidar_answer[13])){
				xp_lidar_H_W_analysis(&lidarDataBuff[13],10*(POINT_NUM/100), 55*(POINT_NUM/100));
				// }else{
				// 	minDis = 0;
				// }
				#else
				memcpy(lidarDataBuff,&get_lidar_answer[13],200*4);
				GaussFilter_new(lidarDataBuff);
				//if(!is_lidar_data_invalid(lidarDataBuff)){

				xp_lidar_data_deal_new(lidarDataBuff, point_count/2 - 110);
				
					//xp_lidar_data_deal_400_point(lidarDataBuff, point_count/2);
				  
			//	}else{
				//	minDis = 0;
			//}
				/*if(!is_lidar_data_invalid(&get_lidar_answer[13])){
					xp_lidar_data_deal_new(&get_lidar_answer[13], point_count/2 - 35);
				}else{
					minDis = 0;
				}*/
				#endif
				
			}
		}
		else if(ret!=0){
			err_cnt++;
			if((online == 1) && (err_cnt > 5)){
				println("lidar offline~~~");
				online=0;
			}
		}
		#ifdef DRYER
		//aos_msleep(500);
		aos_msleep(50);
		#else
      	aos_msleep(50);
		#endif
    }
}

/**
 * @brief 雷达初始化
 * 
 * @return int32_t 0表示初始化成功，非0则失败
 */
int32_t lidar_init(uint8_t port)
{
	int32_t ret = 0;
	
	/* uart port set */
	lidar_uart.port = port;

	/* uart attr config */
	lidar_uart.config.baud_rate	  = 115200;
	lidar_uart.config.data_width   = DATA_WIDTH_8BIT;
	lidar_uart.config.parity 	  = NO_PARITY;
	lidar_uart.config.stop_bits	  = STOP_BITS_1;
	lidar_uart.config.flow_control = FLOW_CONTROL_DISABLED;
	lidar_uart.config.mode		  = MODE_TX_RX;

	/* init uart1 with the given settings */
	ret = hal_uart_init(&lidar_uart);
	if (ret != 0) {
		printf("uart1 init error !\n");
		return -1;
	}

	//让雷达上电连续发的数据改成单点触发
	set_lidar_single_shot();
	set_lidar_single_shot();
	xp_uart_rec_clear(&lidar_uart);
	ret = set_lidar_param();
	/*if(0 != ret){
		return -1;
	}*/
	ret = store_lidar_param();
	/*if(0 != ret){
		return -1;
	}*/
	
	if (0 != aos_task_new("lidar", lidar_data_recv, NULL, 2048)) {
        printf("create lidar thread fail");
		return -1;
    }
		
	return 0;
}


/************************************************************************/
/*
deal lidar recive data
designed by lunanting
param:
	data:data buffer pointer
	len:data length
*/
void xp_lidar_data_deal(u8 *data,u8 len){
	u16 dis=0,min=2000;
	float H=0.0;
	double angle,C,A;
	
	int min_number = 0;
	
	for(u8 i=0;i<len;i++){
		memcpy(&dis,data+i*2,2);

		if(i<50){
			angle=0.54+((49-i)*1.08);
			A=angle*PI/180.0;
			C=cos(A);
			H=C*dis;
		}
		else{
			angle=0.54+((i-50)*1.08);
			A=angle*PI/180.0;
			C=cos(A);
			H=C*dis;
		}
		if(min>H){
			min=(int)H;
			min_number = i;
		}
	}
	minDis=min;
	if(minDis>=1500){
		minDis=0;
	}

	println("lidar min distance=%d",min);
}

static FiLTER_mean maxH_mean={0},minW_mean={0};
/*
analysis hight and width by lidar data
分析雷达数据的高度和宽度
param:
	data:lidar data
	len:point number
*/
void xp_lidar_H_W_analysis(u8 *data,int offset,int len)
{
	u16 dis=0,minW=1500;
	int maxH=-500;
	float H=0.0,W=0.0;
	double angle,C,S,A;
	int half=POINT_NUM/2;
	float a1,a2;
	int usefull=0;

	a1=(float)54.0/POINT_NUM;
	a2=(float)108.0/POINT_NUM;
	
	for(int i=0;i<len;i++){
		memcpy(&dis,data+offset*2+i*2,2);
		//printf("%d ",dis);
		if(dis<2000&&dis>50){
			if(i<(half+LIDAR_dir-offset)){
				if(dis<2000&&dis>50){
					angle=a1+(((half-1+LIDAR_dir-offset)-i)*a2);
					A=angle*PI/180.0;
					C=cos(A);
					W=C*dis;
					S=sin(A);
					H=S*dis;
					usefull++;
				}
			}else{
				if(dis<2000&&dis>50){
					angle=a1+((i-(half-LIDAR_dir-offset))*a2);
					A=angle*PI/180.0;
					C=cos(A);
					W=C*dis;
					S=sin(A);
					H=-1*S*dis;
					usefull++;
				}
	  		}
			if(W<minW){
			minW=(int)W;
			// minDis=minW;
			}
			if(H>maxH){
			maxH=(int)H;
			// maxHigh=maxH;
			}
			// println("point=%d,  dis=%d,  hight=%0.2f,	width=%0.2f",i,dis,H,W);
		}
  }
	//printf("\r\n");
	//println("\r\n point num=%d,  maxH=%dmm,  minW=%dmm,  eff=%0.2f%%\r\n\r\n\r\n",len,maxH,minW,(float)usefull/POINT_NUM*100.0);
	maxHigh=maxH;
	minDis=minW;
}

/*void xp_lidar_H_W_analysis(u8 *data,u8 offset,u8 len){
	u16 dis=0,minW=1500;
	int maxH=-500;
	float H=0.0,W=0.0;
	double angle,C,S,A;
	int vlb=0;

	for(u8 i=0;i<len;i++){
		memcpy(&dis,data+offset*2+i*2,2);
		vlb++;
		// println("point=%d,	dis=%d",i,dis);
		if(dis<2000&&dis>100){
			if(i<(POINT_NUM/2+LIDAR_dir-offset)){
				if(dis<2000&&dis>100){
					angle=(float)(54.0/POINT_NUM)+(((POINT_NUM/2-1+LIDAR_dir-offset)-i)*(float)(108.0/POINT_NUM));
					A=angle*PI/180.0;
					C=cos(A);
					W=C*dis;
					S=sin(A);
					H=S*dis;
				}
			}
			else{
				if(dis<2000&&dis>100){
					angle=(float)(54.0/POINT_NUM)+((i-(POINT_NUM/2-LIDAR_dir-offset))*(float)(108.0/POINT_NUM));
					A=angle*PI/180.0;
					C=cos(A);
					W=C*dis;
					S=sin(A);
					H=-1*S*dis;
				}
			}
			if(W<minW){
				minW=(int)W;
				// minDis=minW;
			}
			if(H>maxH){
				maxH=(int)H;
				// maxHigh=maxH;
			}
			// println("point=%d,	dis=%d,	hight=%0.2f,	width=%0.2f",i,dis,H,W);
		}
	}	
	println("\r\n point num=%d,	maxH=%d,	minW=%d\r\n\r\n\r\n",len,maxH,minW);
	if((minW>=1500))minW=0;
	if(vlb>10){
		maxHigh=maxH;
		minDis=minW;
	}
}*/

//只保留中间30个雷达数据
void xp_lidar_data_deal_new(u8 *data,u16 len){
	u16 dis=0,min=2000;
	float W=0.0,Hight=0.0;
	double angle,C,S,A;
	int maxH=-500;
	int min_number = 0;
	
	for(u16 i=0;i<len;i++){
		#ifdef DRYER
			if(i < 110){
				continue;
			}
		#else
      		if(i < 40){
				continue;
			}
		#endif
		
	
		memcpy(&dis,data+i*2,2);
		
		if(dis>2000 || dis<100){
			continue;
		}
		
		if(i<50*4){
			
			angle=0.54+((49+150-i)*0.27);
			A=angle*PI/180.0;
			C=cos(A);
			W=C*dis;

			S=sin(A);
			Hight=S*dis;
		}
		else{
			angle=0.54+((i-50-150)*0.27);
			A=angle*PI/180.0;
			C=cos(A);
			W=C*dis;
			
			S=sin(A);
			Hight=-1*S*dis;
		}
		if(min>W){
			min=(int)W;
			min_number = i;
		}
		if(Hight < 2000){
			if(Hight>maxH){
				maxH=(int)Hight;
				// maxHigh=maxH;
			}
		}
	}
	minDis=min;
    if((minDis>=1500)||(min==2000)){
		minDis=0;
	}
	maxHigh = maxH;
	
	//println("lidar min distance=%d",min);
}


//只保留中间30个雷达数据
/*void xp_lidar_data_deal_new(u8 *data,u16 len){
	u16 dis=0,min=2000;
	float H=0.0;
	double angle,C,A;
	
	int min_number = 0;
	
	for(u16 i=0;i<len;i++){
		if(i < 110){
			continue;
		}
	
		memcpy(&dis,data+i*2,2);
		
		if(i<50*4){
			//angle=0.54+((49-i)*1.08);
			//angle=0.54+((49+150-i)*0.27);
			angle=0.54+((49+150-i)*0.18);
			A=angle*PI/180.0;
			C=cos(A);
			H=C*dis;
		}
		else{
			//angle=0.54+((i-50-150)*0.27);
			angle=0.54+((i-50-150)*0.18);
			A=angle*PI/180.0;
			C=cos(A);
			H=C*dis;
		}
		if(min>H){
			min=(int)H;
			min_number = i;
		}
	}
	minDis=min;
    if((minDis>=1500)||(min==2000)){
		minDis=0;
	}
	//println("lidar min distance=%d",min);
}*/

//只保留中间30个雷达数据
/*void xp_lidar_data_deal_400_point(u8 *data,u16 len){
	u16 dis=0,min=2000;
	float H=0.0;
	double angle,C,A;
	
	int min_number = 0;
	
	for(u16 i=0;i<len;i++){
		
		memcpy(&dis,data+i*2,2);
		
		if(i<50*4){
			//angle=0.54+((49-i)*1.08);
			angle=0.54+((49+150-i)*0.081);
			A=angle*PI/180.0;
			C=cos(A);
			H=C*dis;
		}
		else{
			angle=0.54+((i-50-150)*0.081);
			A=angle*PI/180.0;
			C=cos(A);
			H=C*dis;
		}
		if(min>H){
			min=(int)H;
			min_number = i;
		}
	}
	minDis=min;
    if((minDis>=1500)||(min==2000)){
		minDis=0;
	}
	//println("lidar min distance=%d",min);
}*/


/*
get lidar min distance
return:distance cm
*/
int xp_lidar_minDis_get(void){
	return minDis;
}


/*
deal lidar max Hight
return:max hight cm
*/
int xp_lidar_hight_deal(void){
	return maxHigh;
}


/*
control lidar open and close
阻塞类型接口，会阻塞 8000 ms 
param:
	sta: 0=close,1=open
*/
void xp_lidar_open_set(int sta){
	int flag=0;

	if(sta==0){
		xp_io_write_pin(1,22,0);
		xp_io_write_pin(1,21,1);
		println("lidar putter closing~~");
		flag=1;
	}
	else{
		xp_io_write_pin(1,21,0);
		xp_io_write_pin(1,22,1);
		println("lidar putter openning~~");
		flag=1;
	}
	if(flag==1){
		aos_msleep(8000);
		println("lidar putter stop~~");
		xp_io_write_pin(1,22,1);
		xp_io_write_pin(1,21,1);
	}
}


/****************************debug*****************************/
/*
flatscan lidar debug
*/
int xp_flatscan_debug(char *type,char *fun,char *param){
	
	if(strcmp(type,"flatscan")==0)return 0;

	else{
		return 0;
	}

	return 1;
}
